#include "AlignAction.h"
#include "math.h"
AlignAction::AlignAction(VisioneController *visione,unsigned short lastPortDepth):ArAction("Align1Action", "Si allinea alla palla") {

	this->visione = visione;
	this->lastPortDepth=lastPortDepth;
	aligned=false;
	myDesired.reset();
}

AlignAction::~AlignAction() {

}

ArActionDesired* AlignAction::fire(ArActionDesired currentDesired){
	double distance = elaborateDistance(lastPortDepth,visione->getProfonditaPalla());

	double beta = 0;
	int gradi = 0;
	int angoloPorta,angoloPalla;
	if(this->aligned==false){
		beta = elaborateBeta(distance,visione);
		gradi = -1;
		angoloPorta = visione->getAngoloPorta();
		angoloPalla = visione->getAngoloPalla();
		if(angoloPorta<angoloPalla)
			gradi = -beta;
		else
			gradi = beta;

		myRobot->setDeltaHeading(gradi);

		std::cout<<"beta: "<<beta<<"\n"<<"distanza: "<<distance<<"\n";
		myRobot->setVel(45);
		myRobot->move(distance);
		this->aligned = true;
	}
	return &myDesired;
}
double AlignAction::elaborateBeta(double d,VisioneController *vision/* unsigned short profPalla*/){
	double beta;
	unsigned short profPalla = vision->getProfonditaPalla();
	double arcArg = (-pow(S,2)+pow(profPalla,2)+pow(d,2))/(2*d*(int)(profPalla));
	if(arcArg>1 || arcArg<-1)
		elaborateBeta(d,vision);
	else{

		beta = acos(arcArg);
		beta = (beta*180)/3.14;

		return beta;
	}
}
double AlignAction::elaborateDistance(unsigned short profPorta,unsigned short profPalla){
	double d;
	double arcArg = (-pow(profPorta,2)+pow(M,2)+pow(profPalla,2))/(2*M*profPalla);
	if(arcArg<-1)
		arcArg=-1;
	else if(arcArg>1)
		arcArg = 1;
	double arcos = acos(arcArg);
	d = sqrt(pow(S,2)+pow(profPalla,2)-2*S*profPalla*cos(180-arcos));
	return d;

}

